#ifndef REMOTE_CONTROL_H
#define REMOTE_CONTROL_H

#include <iostream>
#include "actuatorcontroller.h"
#include "innfos_kinematics.h"
#include <thread>
#include <signal.h>
#include <string.h>
#include <chrono>
#include <math.h>
#include <Eigen/Eigen>
#include <vector>
//#include other .h 

using Vector6=Eigen::Matrix<double,1,6>;
using namespace std;

class remote_control
{
public:	
	//构造和析构函数	
	remote_control();
	~remote_control(); 

	//public members here;
	//including variables and functions you want to call in scripts;
	//position and current value
	double pos[6] = {0, 0, 0, 0, 0, 0};
	double cur[6] = {0, 0, 0, 0, 0, 0};

	//wait time between loops or other situation, in ms.
	void wait_ms(int wait_time);
	//turn pos control mode on
	void poscontrol_on();
	//turn compliance mode on
	void compliance_on();
	//compliance loop for once. you need to write a loop to achieve compliance control.
	void compliance_loop();
	//get the matrix for end-point	
	Eigen::Isometry3d fk_position();

	//end-point following 
    void endpoint_follow(const Eigen::Isometry3d& transform3D);
	
protected:
	//protected members here;
	//normally, most of variables and inside functions are here;

	//initial position value, used for avoiding droping off
	double init_pos[6] = {0, 0, 0, 0, 0, 0};
	//current threshold value
	double curlimit[6] = {100, 0.5, 0.2, 0.15, 0.5, 100};
	//position control ratio
	double posmultiple[6] = {1, 0.5, 0.75, 2, 1, 1};
	//acceleration limit set
	double accellimit[6] = {1200, 500, 1500, 4800, 3600, 1200};
	//velocity limit set
	double vellimit[6] = {1200, 500, 1500, 2400, 1200, 3000};
	//velocity control ratio
	double velratio[6] = {1, 800, 1200, 2500, 1, 1};
	//actuators for current control
	int curcontrol[2] = {0, 4};
	//actuators for profile pos control
	int poscontrol[] = {};
	//actuators for profile vel control
	int velcontrol[4] = {1, 2, 3, 5};
	//gravity compensation value
	double gravity[6] = {0, 0, 0, 0, 0, 0};
	//weight for joints
	double mj = 0.3;
	//weight for arm
	double ma = 0.17;
	//weight for T connector
	double mt = 0.124;
	//arm length
	double l = 0.173;
	//length between joint 4 to 5
	double ls = 0.0792;
	//gravity constant
	double g = 9.81;
	//torque over current
	double torqueratio = 1.8;
	//current filter buffer
	double cur_filter[6][3] = {{0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}};

	//class point
	ActuatorController * pController;
	//actuator array
	std::vector<ActuatorController::UnifiedID> uIDArray;
	//innfos_kinematics
	innfos_kinematics ik;


private:
	//private members here;
};

#endif
